#include "jakarobotdevice.h"
#include <QSequentialIterable>
#include <QCoreApplication>
#include <QThreadPool>
#include "../task/JakaMoveTask.h"

#define PI 3.1415926

JakaRobotDevice::JakaRobotDevice(QObject *parent)
    : Device{parent}
    , m_strIP("192.168.1.1")
    , m_nPort(1000)
{

}

Device* JakaRobotDevice::clone(Device* pDevice){
    m_strID = pDevice->getStrID();

    m_strIP = ((JakaRobotDevice*)pDevice)->GetIP();
    m_nPort = ((JakaRobotDevice*)pDevice)->GetPort_();
    return this;
}

bool JakaRobotDevice::isConnect(){
    RobotStatus robotStatus;
    m_robot.get_robot_status(&robotStatus);
    return robotStatus.is_socket_connect;
}

bool JakaRobotDevice::isPower(){
    RobotStatus robotStatus;
    m_robot.get_robot_status(&robotStatus);
    return robotStatus.powered_on;
}

bool JakaRobotDevice::isEnabled(){
    RobotStatus robotStatus;
    m_robot.get_robot_status(&robotStatus);
    return robotStatus.enabled;
}

bool JakaRobotDevice::connect(QString strIP){
    if(strIP == "")
        strIP = m_strIP;

    errno_t err = m_robot.login_in(m_strIP.toLatin1());
    if(ERR_SUCC != err){
        return false;
    }
    return true;
}

bool JakaRobotDevice::power(bool bEnabled){
    RobotStatus robotStatus;
    m_robot.get_robot_status(&robotStatus);
    errno_t err;
    if(bEnabled){
        if(!robotStatus.powered_on){
            err = m_robot.power_on();
            if(ERR_SUCC != err){
                return false;
            }
        }
    }else{
        if(robotStatus.powered_on){
            err = m_robot.power_off();
            if(ERR_SUCC != err){
                return false;
            }
        }
    }

    return true;
}

bool JakaRobotDevice::enable(bool bEnabled){
    RobotStatus robotStatus;
    m_robot.get_robot_status(&robotStatus);
    errno_t err;
    if(bEnabled){
        if(!robotStatus.enabled){
            err = m_robot.enable_robot();
            if(ERR_SUCC != err){
                return false;
            }
        }
    }else{
        if(robotStatus.enabled){
            err = m_robot.disable_robot();
            if(ERR_SUCC != err){
                return false;
            }
        }
    }

    return true;
}

int JakaRobotDevice::initDevice(){
    errno_t err = m_robot.login_in(m_strIP.toLatin1());
    if(ERR_SUCC != err){
        return -1;
    }

    qDebug() << "连接机械臂";
    //延时一秒
    QTime dieTime = QTime::currentTime().addMSecs(1000);
    while (QTime::currentTime() < dieTime)
        QCoreApplication::processEvents(QEventLoop::AllEvents, 100);

    RobotStatus robotStatus;
    m_robot.get_robot_status(&robotStatus);
    if(!robotStatus.powered_on){
        qDebug() << "Jaka Connect Success!";
        err = m_robot.power_on();
        if(ERR_SUCC != err){
            return -1;
        }
        qDebug() << "Jaka Power On Success!";
    }else{
        qDebug() << "Jaka already Power On!";
    }

    qDebug() << "机械臂上电";

    //延时一秒
    dieTime = QTime::currentTime().addMSecs(1000);
    while (QTime::currentTime() < dieTime)
        QCoreApplication::processEvents(QEventLoop::AllEvents, 100);

    if(!robotStatus.enabled){
        err = m_robot.enable_robot();
        if(ERR_SUCC != err){
            return -1;
        }
        qDebug() << "Jaka Enable Success!";
    }
    else{
        qDebug() << "Jaka already Enabled!";
    }

    qDebug() << "机械臂使能";
    return 0;
}

bool JakaRobotDevice::isInPos(JointValue dest_pos,JointValue cur_pos,double theshold){
    double sum = 0;
    for(int i = 0;i < 6;i ++){
        sum += qAbs(dest_pos.jVal[i] - cur_pos.jVal[i]);
    }
    return sum < theshold ? true : false;
}

#ifdef MULTITHREAD
void JakaRobotDevice::writeData(const QVariant &data)
{
    QMap<QString,QVariant> map;

    if (data.canConvert<QVariantList>()) {
        QList<QVariant> list = data.value<QList<QVariant>>();

        map.insert("dblJoint1",             list[0].toDouble());
        map.insert("dblJoint2",             list[1].toDouble());
        map.insert("dblJoint3",             list[2].toDouble());
        map.insert("dblJoint4",             list[3].toDouble());
        map.insert("dblJoint5",             list[4].toDouble());
        map.insert("dblJoint6",             list[5].toDouble());
        map.insert("dblVelocity",           list[6].toDouble());
        map.insert("dblAcceleration",       list[7].toDouble());
        map.insert("pRobot",                QVariant::fromValue((void *) &m_robot));

        JakaMoveTask* pTask = new JakaMoveTask();
        pTask->SetParameters(map);
        QThreadPool::globalInstance()->start(pTask);
    }
}
#else
void JakaRobotDevice::writeData(const QVariant &data)
{
    JointValue cur_jval {0};
    qDebug() << "Jaka Robot WriteData";
    double velocity = 2;
    double acceleration = 3;

    if (data.canConvert<QVariantList>()) {
        QSequentialIterable iterable = data.value<QSequentialIterable>();
        //foreach方式遍历:
        int index = 0;
        foreach (const QVariant &v, iterable) {
            if(index == 6){
                velocity = v.toDouble();
            }else if(index == 7){
                acceleration = v.toDouble();
            }
            else{
                m_joint_pos.jVal[index] = PI * v.toDouble() / 180.0;
            }
            index++;
        }

        qDebug() << "velocity";
        qDebug() << QString::number(velocity);
        // 非阻塞
        m_robot.joint_move(&m_joint_pos,ABS,FALSE,velocity);//velocity : 1 代表速度为 1rad/s; 机械臂关节运动 ，关节速度上限 180deg/s
        while(true){
            m_robot.get_joint_position(&cur_jval);
            QCoreApplication::processEvents(QEventLoop::AllEvents, 10);
            if(isInPos(m_joint_pos,cur_jval,0.01)){
                break;
            }
        }
        // 阻塞
    }
}
#endif

void JakaRobotDevice::uninitDevice(){
    errno_t err;

    RobotStatus robotStatus;
    m_robot.get_robot_status(&robotStatus);
    if(robotStatus.enabled){
        qDebug() << "Jaka Connect Success!";
        err = m_robot.disable_robot();;
    }

    if(robotStatus.powered_on){
        err = m_robot.power_off();
    }
}

void JakaRobotDevice::writeXML(QXmlStreamWriter &writer)
{
    // 写入当前操作的信息
    writer.writeStartElement("JakaRobotDevice");
    writer.writeAttribute(QStringLiteral("id"), m_strID);

    writer.writeTextElement("ip", m_strIP);
    writer.writeTextElement("port", QString::number(m_nPort));
    writer.writeEndElement(); // 结束当前操作的元素
}

void JakaRobotDevice::readFromXML(QXmlStreamReader &reader)
{
    QXmlStreamAttributes attributes = reader.attributes();
    if (attributes.hasAttribute("id")) {
        m_strID = attributes.value("id").toString();
    }

    QXmlStreamReader::TokenType token = reader.readNext();
    while (!(reader.tokenType() == QXmlStreamReader::EndElement
             && reader.name() == QStringLiteral("JakaRobotDevice"))) {
        if (reader.name() == QStringLiteral("ip")) {
            m_strIP = reader.readElementText();
        } else if (reader.name() == QStringLiteral("port")) {
            m_nPort = reader.readElementText().toInt();
        }

        reader.readNext();
    }
}

QString JakaRobotDevice::getClassName()
{
    return this->metaObject()->className();
}

QString JakaRobotDevice::GetIP() const
{
    return m_strIP;
}

void JakaRobotDevice::SetIP(const QString &newStrIP)
{
    if (m_strIP == newStrIP)
        return;
    m_strIP = newStrIP;
    emit strIPChanged();
}

int JakaRobotDevice::GetPort_() const
{
    return m_nPort;
}

void JakaRobotDevice::SetPort_(int newNPort)
{
    if (m_nPort == newNPort)
        return;
    m_nPort = newNPort;
    emit nPortChanged();
}

void JakaRobotDevice::SetDigitalOutput(int index,int value){
    m_robot.set_digital_output(IO_CABINET,index,value);
}

